/***************************************************************************
 *            dc1394cam_driver.cpp
 *  
 *  Wed Oct 21 11:25:00 2009
 *  Copyright  20010  Michael Warren
 *  Queensland University of Technology, CyPhy Lab
 *  <michael.warren@qut.edu.au>
 ****************************************************************************/
#include "dc1394cam_driver.h"

//TODO: Remove this when debugging is complete
#include <iostream>
using namespace std;

// This is copied from the CV driver, not sure
// exactly what it does, but it looks important
struct DC1394_module
{
    DC1394_module();
    ~DC1394_module();

    dc1394_t* dc;
    fd_set camFds;
};

DC1394_module::DC1394_module()
{
    dc = dc1394_new();
    FD_ZERO(&camFds);
}

DC1394_module::~DC1394_module()
{
    if (dc)
        dc1394_free(dc);
    dc = 0;
}

static DC1394_module dc1394;


// IIDC 1.32 Feature Types
const char *featuresStr[]={ 
    "Brightness" ,
    "Exposure",
    "Sharpness", 
    "White Balance", 
    "Hue", 
    "Saturation",
    "Gamma",
    "Shutter",
    "Gain",
    "Iris",
    "Focus",
    "Temperature",
    "Trigger",
    "TriggerDelay",
    "WhiteShading",
    "FrameRate",
    "Zoom",
    "Pan",
    "Tilt",
    "OpticalFilter",
    "CaptureSize",
    "CaptureQuality"
};

// Point Grey Feature Types
const char *ptGreyFeaturesStr[]={ 
    "MaxShutter"
};
// Update this value to reflect the size of ptGreyFeaturesStr.
// Not exactly pretty, but avoids some messed up code
unsigned ptGreyFeaturesStrSize = 1;


// Check the user desired color mode against the one returned by camera
bool checkColorMode(string format, dc1394color_coding_t coding) {
    bool result = false;
    if (format == "RGB" && (coding == DC1394_COLOR_CODING_RGB8 ||
                            coding == DC1394_COLOR_CODING_RGB16 ))
        result = true;
    else if (format == "YUV" && (coding == DC1394_COLOR_CODING_YUV411 ||
                                 coding == DC1394_COLOR_CODING_YUV422 ||
                                 coding == DC1394_COLOR_CODING_YUV444))
        result = true;
    else if (format == "GREY" && (coding == DC1394_COLOR_CODING_MONO8 ||
                                  coding == DC1394_COLOR_CODING_MONO16 ))
        result = true;
    else if (format == "RGB8" && coding == DC1394_COLOR_CODING_RGB8 )
        result = true;  
    else if (format == "RGB16" && coding == DC1394_COLOR_CODING_RGB16 )
        result = true;
    else if (format == "YUV411" && coding == DC1394_COLOR_CODING_YUV411)
        result = true;
    else if (format == "YUV422" && coding == DC1394_COLOR_CODING_YUV422)
        result = true;
    else if (format == "YUV444" && coding == DC1394_COLOR_CODING_YUV444)
        result = true;
    else if (format == "GREY8" && coding == DC1394_COLOR_CODING_MONO8)
        result = true;
    else if (format == "GREY16" && coding == DC1394_COLOR_CODING_MONO16)
        result = true;
    else if (format == "RAW8" && coding == DC1394_COLOR_CODING_RAW8)
        result = true;
    else if (format == "RAW16" && coding == DC1394_COLOR_CODING_RAW8)
        result = true;
    else
        result = false;
    return result;
}


bool isColorValid(std::string colorMode) {
    if (!(colorMode == "RGB" || colorMode == "YUV" || colorMode == "GREY" || 
        colorMode == "RGB8" || colorMode == "RGB16" || colorMode == "YUV411" || 
        colorMode == "YUV422" || colorMode == "YUV444" || colorMode == "GREY8" || 
        colorMode == "GREY16"  || colorMode == "RAW8"  || colorMode == "BayerRG8" ||
        colorMode == "BayerGR8" || colorMode == "BayerGB8" || colorMode == "BayerBG8"
        )) {
        std::cerr << "dc1394cam_driver Error: Cannot support color mode \"" << colorMode << "\""
        << " dc1394cam_driver currently supports \"RGB\", \"RGB8\", \"RGB16\", \"YUV\", \"YUV411\","
        << "\"YUV422\", \"YUV444\", \"GREY\", \"GREY8\", \"GREY16\", \"RAW8\", \"BayerRG8\", \"BayerGR8\","
        <<  "\"BayerGB8\" and \"BayerBG8\"" << endl;
        return false;
    }
    else {
        return true;
    }
}    



// Abstraction for asking the camera for register information
static uint32_t getControlRegister(dc1394camera_t *camera, uint64_t offset)
{
    uint32_t value = 0;
    dc1394error_t err = dc1394_get_control_register(camera, offset, &value);

    assert(err == DC1394_SUCCESS);
    return err == DC1394_SUCCESS ? value : 0xffffffff;
}

camCapture::camCapture()
{
    // Set defaults
    guid = 0;
    dcCam = 0;
    isoSpeed = 800;
    fps = "15";
    fps_float = 15;
    nDMABufs = 8;
    started = false;
    cameraId = 0;
    frameWidth = 640;
    frameHeight = 480;
    index = 0;
    format7 = false;
    prefColorMode = "RGB";
    colorMode = (dc1394color_coding_t)NULL;
#ifdef DC1394CAM_DRIVER_OPENCV_INSTALLED
    img = 0;
#endif //DC1394CAM_DRIVER_OPENCV_INSTALLED
    timestamp = 0;
    prevtimestamp = 0;
    frameC = 0;
    cleanup = false;
    suppress_warnings = false;
    opened = false;
    isColor = false;
    frames_behind = 0;
}

// Check config and start capturing
bool camCapture::startCapture()
{
    //std::cout<< "startcapture()..." << endl;
    int i;
    int code = 0;
    if (!opened) {
        std::cerr << "dc1394cam_driver Error: Camera not yet opened. This is probably due"
            << " to a previous setup problem." << endl;
        return 0;
    }
    if (cleanup) {
        uint32_t val;
        if ( dc1394_video_get_bandwidth_usage(dcCam, &val) == DC1394_SUCCESS &&
                dc1394_iso_release_bandwidth(dcCam, val) == DC1394_SUCCESS )
            std::cout << "dc1394cam_driver Info: Succesfully released " << val << " bytes of Bandwidth." << endl;
        if ( dc1394_video_get_iso_channel(dcCam, &val) == DC1394_SUCCESS &&
                dc1394_iso_release_channel(dcCam, val) == DC1394_SUCCESS )
            std::cout << "dc1394cam_driver Info: Succesfully released ISO channel #" << val << "." << endl;
    }
    cleanup = false;
    
    // Set ISO Mode
    if (isoSpeed > 400) {
        code = dc1394_video_set_operation_mode(dcCam, DC1394_OPERATION_MODE_1394B);
        if (code != DC1394_SUCCESS) {
            std::cerr << "dc1394cam_driver Error: Could not set 1394b mode for requested ISO speed of " 
            << isoSpeed << "Mbps. Do your firewire bus and camera support it?" << endl;
            return code >= 0;
        }
    }
    
    // Set ISO speed
    if (isoSpeed > 0) {
        code = dc1394_video_set_iso_speed(dcCam,
                                          isoSpeed <= 100 ? DC1394_ISO_SPEED_100 :
                                          isoSpeed <= 200 ? DC1394_ISO_SPEED_200 :
                                          isoSpeed <= 400 ? DC1394_ISO_SPEED_400 :
                                          isoSpeed <= 800 ? DC1394_ISO_SPEED_800 :
                                          isoSpeed == 1600 ? DC1394_ISO_SPEED_1600 :
                                          DC1394_ISO_SPEED_3200);
        if (code != DC1394_SUCCESS) {
            std::cerr << "dc1394cam_driver Error: Could not set ISO speed of " 
            << isoSpeed << "Mbps. Do your firewire bus and camera support it?" << endl;
            return code >= 0;
        }
    }
    
    // Make sure color mode is something that driver understands
    if (!isColorValid(prefColorMode)) {
        return 0;
    }
   
    // Set video mode and size
    dc1394video_mode_t bestMode = (dc1394video_mode_t)NULL;
    if (frameWidth > 0 || frameHeight > 0)
    {
        dc1394video_modes_t videoModes;
        // Get all supported video modes
        dc1394_video_get_supported_modes(dcCam, &videoModes);
        dc1394color_coding_t coding;
        // If we need Format_7
        if (prefColorMode== "RAW8" || prefColorMode== "BayerRG8" || prefColorMode== "BayerGR8" 
         || prefColorMode== "BayerGB8" || prefColorMode== "BayerBG8") {
            //Searching for a Format 7 Mode that supports RAW8
            //std::cout << "Searching for Format_7 Mode 0" << endl;
            for (int i = videoModes.num - 1; i >= 0; i--)
            {
                // FORMAT 7 modes (i.e. "scalable")
                if (dc1394_is_video_mode_scalable(videoModes.modes[i]))
                {
                    //cout << "Format_7 Mode " << (videoModes.modes[i] - DC1394_VIDEO_MODE_FORMAT7_MIN) << " detected" << endl;
                    dc1394_get_color_coding_from_video_mode(dcCam, videoModes.modes[i], &coding);
                    // Is code suitable for our color mode selection?
                    if (checkColorMode(prefColorMode,coding))
                    {
                        if (videoModes.modes[i] == DC1394_VIDEO_MODE_FORMAT7_0)
                        {
                            bestMode = videoModes.modes[i];
                            colorMode = coding;
                            format7 = true;
                            break;
                        }
                    }
                }
            }
        } else {
            // Not Format_7
            for (int i = videoModes.num - 1; i >= 0; i--)
            {
                // don't consider FORMAT 7 modes (i.e. "scalable"), just native camera modes
                if (!dc1394_is_video_mode_scalable(videoModes.modes[i]))
                {
                    dc1394_get_color_coding_from_video_mode(dcCam, videoModes.modes[i], &coding);
                    // Is code suitable for our color mode selection?
                    if (checkColorMode(prefColorMode,coding))
                    {
                        dc1394video_mode_t mode = videoModes.modes[i];
                        unsigned int width, height;
                        dc1394_get_image_size_from_video_mode(dcCam, mode, &width, &height);

                        if ((width == frameWidth) && (height == frameHeight))
                        {
                            bestMode = videoModes.modes[i];
                            colorMode = coding;
                            break;
                        }
                    }
                }
            }
        }
    }
    
    //Make sure we found a good mode
    if (!bestMode) {
        std::cerr << "dc1394cam_driver Error: Could not find a configuration for your camera that meets the requirements: "
            << frameWidth << "x" << frameHeight << ", " << prefColorMode << ". Does your camera support that color mode and in that"
            << " resolution?" << endl;
        return 0;
    }
    
    //Check for non-default feature settings in config file
    int featureIndex = 0;
    // For all dc1394 features: check if we have a user-defined setting, 
    // otherwise force the feature to behave automatically
    for(dc1394feature_t feature = DC1394_FEATURE_MIN; feature <= DC1394_FEATURE_MAX; 
            feature = (dc1394feature_t)(feature+1)) { 
        // Check for a user defined setting for features
        std::map<std::string,int>::iterator it;
        it = features_to_set.find(featuresStr[featureIndex]);
        // Check for a user defined setting for absolute features
        std::map<std::string,float>::iterator it_abs;
        it_abs = features_abs_to_set.find(featuresStr[featureIndex]);
        if (it != features_to_set.end()) {// If the feature has been set by the user
            setDC1394Feature(dcCam, feature, featuresStr[featureIndex], DC1394_FEATURE_MODE_MANUAL, it->second);
        } else if (it_abs != features_abs_to_set.end()) { // If the absolute feature has been set by the user
            setDC1394FeatureAbs(dcCam, feature, featuresStr[featureIndex], DC1394_FEATURE_MODE_MANUAL, it_abs->second);
        } else { // Otherwise, force the feature to manual mode
            setDC1394Feature(dcCam, feature, featuresStr[featureIndex], DC1394_FEATURE_MODE_AUTO, 0);
        }
        featureIndex++;
    }
    
    // For all Point Grey features: check if we have a user-defined setting, 
    // otherwise force the feature to behave automatically
    for(unsigned idx = 0; idx < ptGreyFeaturesStrSize; idx++) { 
        // Check for a user defined setting for features
        std::map<std::string,int>::iterator it;
        it = ptgrey_features_to_set.find(ptGreyFeaturesStr[idx]);
	    //std::cout << "ptgrey_features_to_set.size(): " << ptgrey_features_to_set.size() << std::endl;
        // Check for a user defined setting for absolute features
        std::map<std::string,float>::iterator it_abs;
        it_abs = ptgrey_features_abs_to_set.find(ptGreyFeaturesStr[idx]);
	    //std::cout << "ptgrey_features_abs_to_set.size(): " << ptgrey_features_abs_to_set.size() << std::endl;
        if (it != ptgrey_features_to_set.end()) {// If the feature has been set by the user
	        //std:: cout << "Setting point grey feature " << std::endl;
            setPtGreyFeature(ptGreyFeaturesStr[idx],it->second);
        } else if (it_abs != ptgrey_features_abs_to_set.end()) { // If the absolute feature has been set by the user
	        //std:: cout << "reSetting point grey absolute feature " << it_abs->first << " to " << it_abs->second << std::endl;
            setPtGreyFeatureAbs(ptGreyFeaturesStr[idx], it_abs->second);
        } else { // Otherwise, force the feature to auto mode
            setPtGreyFeatureAuto(ptGreyFeaturesStr[idx]);
        }
        featureIndex++;
    }
    

    if (!format7) {
        // Normal colour
        
        // Check framerates
        dc1394framerate_t bestFps;
        dc1394video_mode_t mode;
        dc1394framerates_t framerates;
        dc1394framerate_t framerate;
        if (fps == "1.875")
            framerate = DC1394_FRAMERATE_1_875;
        else if (fps == "3.75")
            framerate = DC1394_FRAMERATE_3_75;
        else if (fps == "7.5")
            framerate = DC1394_FRAMERATE_7_5;
        else if (fps == "15")
            framerate = DC1394_FRAMERATE_15;
        else if (fps == "30")
            framerate = DC1394_FRAMERATE_30;
        else if (fps == "60")
            framerate = DC1394_FRAMERATE_60;
        else {
            std::cerr << "dc1394cam_driver Error1: Cannot support framerate of \"" << fps << "\" fps."
                << " dc1394cam_driver supports only framerates of 1.875, 3.75, 7.5, "
                << "15, 30 or 60 fps" << endl;
        }
        double minDiff = DBL_MAX;
        bestFps = (dc1394framerate_t) - 1;

        // For the selected color mode and resolution, find which framerates are supported
        if (dc1394_video_get_supported_framerates(dcCam, bestMode, &framerates) != DC1394_SUCCESS)
        {
            std::cerr << "dc1394cam_driver Error: Can't get a framerate associated with requested mode."
                << " Does your camera support this combination?" << endl;
            return 0;
        }
        // Find the closest available framerate to the one requested and set it
        for (int i = 0; i < (int)framerates.num; i++)
        {
            dc1394framerate_t ifps = framerates.framerates[i];
            double diff = fabs(ifps - framerate);
            if (diff < minDiff)
            {
                minDiff = diff;
                bestFps = ifps;
            }
        }
        if (bestFps != framerate) {
            std::cerr << "dc1394cam_driver Warning: Could not find a framerate for your camera that meets the resolution "
                << "and color mode requirements: "<< frameWidth << "x" << frameHeight << ", " << prefColorMode << " @"
                << fps << " fps. Selecting closest available framerate (DC1394 value: " << bestFps << ")." << endl;
        }
        // Check for color
        isColor =  colorMode != DC1394_COLOR_CODING_MONO8 &&
                    colorMode != DC1394_COLOR_CODING_MONO16 &&
                    colorMode != DC1394_COLOR_CODING_MONO16S;
            
        // Warn the user if the buffer will exceed 32MB, problems may occur otherwise
        if ((unsigned)nDMABufs*(unsigned)frameWidth*(unsigned)frameHeight*(isColor ? 3 : 1) > 32e6) {
            std::cerr << "dc1394cam_driver Warning: Your nDMABufs buffer may be too big for the desired frame size."
                << " Make it smaller if you have issues with setting up capture." << endl;
        }

        // Set frame size and colour coding
        if (dc1394_video_set_mode(dcCam, bestMode) != DC1394_SUCCESS) {
            std::cerr << "dc1394cam_driver Error: Could not set video mode." << endl;
            return 0;
        }
        // Set framerate
        if (dc1394_video_set_framerate(dcCam, bestFps) != DC1394_SUCCESS) {
            std::cerr << "dc1394cam_driver Error: Could not set framerate." << endl;
            return 0;
        }
        // Set up the camera
        code = dc1394_capture_setup(dcCam, nDMABufs, DC1394_CAPTURE_FLAGS_DEFAULT);
        if (code >= 0)
        {
            FD_SET(dc1394_capture_get_fileno(dcCam), &dc1394.camFds);
            // Start transmission
            dc1394_video_set_transmission(dcCam, DC1394_ON);  
            //std::cout << "camcapture set transmission" << endl;      
        }
        else {
            std::cerr << "dc1394cam_driver Error: Could not set up camera." << endl;
            return code;
        }
        if (code == DC1394_SUCCESS) started = true;
        //std::cout << "startcapture started... " << started << endl;
        return code >= 0;
    } else {
        // Format_7
        // camera status, speed and operationg mode
        int status;
        
        std::cout << "dc1394cam_driver Warning: Format7 (or RAW8) mode has poor behaviour, particularly when used with more "
                << "than once camera, including improper frame rates and bad synchronisation signals." << std::endl;

        dc1394video_mode_t mmode;
        if (dc1394_video_get_mode(dcCam, &mmode) != DC1394_SUCCESS)
        {
            std::cerr << "dc1394cam_driver Error: Unable to get current video mode." << endl;
            return 0;
        }
        else
        {
            //std::cout  << "Current mode:" << mmode << endl;
        }
        // setup capture
        if (dc1394_video_set_mode(dcCam, bestMode) != DC1394_SUCCESS)
        {
            std::cerr << "dc1394cam_driver Error: Unable to set video mode." << endl;
            return 0;
        }

        if (dc1394_video_get_mode(dcCam, &bestMode) != DC1394_SUCCESS)
        {
            std::cerr << "dc1394cam_driver Error: Unable to retrieve set mode." << endl;
            return 0;
        }

        unsigned int max_width, max_height;
        
        if (dc1394_format7_get_max_image_size(dcCam, bestMode, &max_width, &max_height) != DC1394_SUCCESS)
        {
            std::cerr << "dc1394cam_driver Error: Unable to query max image size." << endl;
            return 0;
        }
        else
        {
            //std::cout << "Supported MAX width:" << max_width << " max_height:" << max_height << endl;
        }

        unsigned int width, height;

        if (dc1394_format7_get_image_size(dcCam, bestMode, &width, &height) != DC1394_SUCCESS)
        {
            std::cerr << "dc1394cam_driver Error: Unable to query image size." << endl;
            return 0;
        }
        else
        {
            //std::cout << "Current image width:" << width << " height:" << height << endl;
        }

        unsigned int min_bytes, max_bytes;
        if (dc1394_format7_get_packet_parameters(dcCam, bestMode, &min_bytes, &max_bytes) != DC1394_SUCCESS)
        {
            //std::cerr << "dc1394cam_driver Error: Unable to query packet size." << endl;
            return 0;
        }
        else
        {
            //std::cout << "Supported packet size MIN:" << min_bytes << " MAX:" << max_bytes << endl;
        }
        
        // Figure out the necessary packet size for a particular frame rate
        double bus_period = 0;
        
        dc1394speed_t iso_val = isoSpeed <= 100 ? DC1394_ISO_SPEED_100 :
                                          isoSpeed <= 200 ? DC1394_ISO_SPEED_200 :
                                          isoSpeed <= 400 ? DC1394_ISO_SPEED_400 :
                                          isoSpeed <= 800 ? DC1394_ISO_SPEED_800 :
                                          isoSpeed == 1600 ? DC1394_ISO_SPEED_1600 :
                                          DC1394_ISO_SPEED_3200;
        switch (iso_val) {
            case DC1394_ISO_SPEED_100:
                bus_period = 500e-6;
            break;
            case DC1394_ISO_SPEED_200:
                bus_period = 250e-6;
            break;
            case DC1394_ISO_SPEED_400:
                bus_period = 125e-6;
            break;
            case DC1394_ISO_SPEED_800:
                bus_period = 62.5e-6;
            break;
            default:
                bus_period = 500e-6;
            break;
        }
        // Calculate the number of packets transmitted per frame period
        int num_packets = (int) (1.0/(bus_period*fps_float) + 0.5);
        // A magic multiplier that is required to obtained the desired frame rate
        int magic_number = 17; 
        // Determine the required packet size
        int denominator = num_packets*8; 
        int packet_size = (width*height*magic_number + denominator - 1)/denominator;
        //std::cout << "Requested packet size: " << packet_size << endl;
        // Set the region of interest at the centre of the image
        unsigned left = (max_width - width)/2; 
        unsigned top = (max_height - height)/2;
        // Set the region of interest, packet size and by association, the frame rate. In Format7 mode, framerate is dependant on packet size in a convoluted way.
        status = dc1394_format7_set_roi(dcCam, bestMode, DC1394_COLOR_CODING_RAW8, packet_size, left, top, frameWidth, frameHeight); //9500
        if (status != DC1394_SUCCESS)
        {
            std::cerr << "dc1394cam_driver Error: Unable to set ROI." << endl;
            return 0;
        }

        unsigned int bytes_per_packet;
        dc1394color_coding_t color_coding;

        status = dc1394_format7_get_roi(dcCam, bestMode, &color_coding, &bytes_per_packet, &left, &top, &width, &height);
        if (status != DC1394_SUCCESS)
        {
            std::cerr << "dc1394cam_driver Error: Unable to get ROI." << endl;
            return 0;
        }
        else
        {
            //std::cout << "Current setup bytes per packet:" << bytes_per_packet << " ROI (left,top,width,height): (";
            //std::cout << left << "," << top << "," << width << "," << height << ")" << endl;
        }

    unsigned int packet_bytes;
    if (dc1394_format7_get_packet_size(dcCam, bestMode, &packet_bytes) != DC1394_SUCCESS)
        {
            std::cerr << "dc1394cam_driver Error: Unable to query packet size." << endl;
            return 0;
        }
        else
        {
            //std::cout << "Current packet size: " << packet_bytes << endl;
        }

        // check the image size in bytes
        uint64_t totalbytes;
        status = dc1394_format7_get_total_bytes(dcCam, bestMode, &totalbytes);
        if (status != DC1394_SUCCESS)
        {
            std::cerr << "dc1394cam_driver Error: Cannot check image size." << endl;
            return 0;
        }
        else
        {
            //std::cout << "Bytes per frame:" << totalbytes << endl;
        }

        // Set up the camera
        code = dc1394_capture_setup(dcCam, nDMABufs, DC1394_CAPTURE_FLAGS_DEFAULT);
        if (code >= 0)
        {
            FD_SET(dc1394_capture_get_fileno(dcCam), &dc1394.camFds);
            // Start transmission
            dc1394_video_set_transmission(dcCam, DC1394_ON);  
            //std::cout << "camcapture set transmission" << endl;      
        }
        else {
            std::cerr << "dc1394cam_driver Error: Could not set up camera." << endl;
            return code;
        }
        if (code == DC1394_SUCCESS) started = true;
	    //std::cout << "startcapture started... " << started << endl;
        return code >= 0;
    }
}


// Check that camera is available on bus and claim it
bool camCapture::open(int idx)
{
    //std::cout << "camcapture: opening..." << endl;
    index = idx;
    bool result = false;
    dc1394camera_list_t* cameraList = 0;
    dc1394error_t err;

    if (!dc1394.dc)
        goto _exit_;

    err = dc1394_camera_enumerate(dc1394.dc, &cameraList);
    
    if (err < 0 ) {
        std::cerr << "dc1394cam_driver Error: Cannot enumerate cameras." << endl;       
        goto _exit_;
    }
    if (!cameraList) {
        std::cerr << "dc1394cam_driver Error: cameraList is empty." << endl;       
        goto _exit_;
    }
    if ((unsigned)index >= (unsigned)cameraList->num) {
        std::cerr << "dc1394cam_driver Error: Cannot connect to camera " << index
            << ", only " << cameraList->num << " cameras on bus." << endl;       
        goto _exit_;
    }

    if (guid) {
        // User has set a camera guid to attempt to connect to
        for (unsigned int i = 0; i < cameraList->num; i++) {
            // Make sure camera with guid exists
            if (guid == cameraList->ids[i].guid) {
                dcCam = dc1394_camera_new(dc1394.dc, guid);
                index = i;
                break;
            }
        }
        if (!dcCam) {
            std::cerr << "dc1394cam_driver Error: Camera with GUID \"" << guid
            << "\" doesn't seem to exist on the bus or may already be in use." << endl;
            goto _exit_;
        }
    }
    else {
        // User doesn't care which camera, connect to available index on bus
        //std::cout << "camcapture::open() index: " << index << endl;
        guid = cameraList->ids[index].guid;
        std::cout << "GUID[" << index << "]: " << guid << endl;
        dcCam = dc1394_camera_new(dc1394.dc, guid);
    }
    if (!dcCam) {
        std::cerr << "dc1394cam_driver Error: Error connecting to Camera with GUID \"." << guid << endl;
        goto _exit_;
    }

    if (dc1394_camera_reset(dcCam) != DC1394_SUCCESS) {
        std::cerr << "dc1394cam_driver Error: Error forcing reset on camera with GUID \"." << guid << endl;
        goto _exit_;
    }
    
    cameraId = dcCam->vendor_id;

    result = true;
    opened = true;
    //std::cout << "camcapture: finished opening" << endl;
_exit_:
    if (cameraList)
        dc1394_camera_free_list(cameraList);
    return result;
}

// Cleanup memory and close firewire camera cleanly
void camCapture::close()
{
    if (dcCam)
    {
        if (FD_ISSET(dc1394_capture_get_fileno(dcCam), &dc1394.camFds))
            FD_CLR(dc1394_capture_get_fileno(dcCam), &dc1394.camFds);
        dc1394_video_set_transmission(dcCam, DC1394_OFF);
        dc1394_capture_stop(dcCam);
        dc1394_camera_free(dcCam);
        dcCam = 0;
        started = false;
    }
#ifdef DC1394CAM_DRIVER_OPENCV_INSTALLED
    if (img) {
        //std::cout << "RELEASING IMAGE" << endl;
        //cvReleaseImage(&img);
    }
#endif //DC1394CAM_DRIVER_OPENCV_INSTALLED
    if (frameC)
    {
        if (frameC->image)
            free(frameC->image);
        free(frameC);
        frameC = 0;
    }
    opened = false;
}

// Dequeue a single frame from dc1394 buffer
bool camCapture::grabFrame()
{
    // Capture policy is to wait until a frame arrives in the buffer
    dc1394capture_policy_t policy = DC1394_CAPTURE_POLICY_WAIT;
    bool code = false;
    dc1394video_frame_t *dcFrame = 0;
    int i, nch;
    //std::cout << "grabFrame: opening..."<< endl;
    // If the process hasn't opened up the camera, do it for them quietly
    if (!opened) {
        //std::cout << "grabFrame: !opened" << endl;
        if (!open()) {
            //std::cout << "grabFrame: closing..." << endl;
            close();
            return false;
        }
    }
    //std::cout << "grabFrame: finished opening..."<< endl;
    // Make sure the cameras are set up and capturing
    if (!dcCam || (!started && !startCapture()))
        return false;
    //std::cout << "grabFrame: finished starting..."<< endl;
    // Grab the frame from the dc1394 buffer
    //std::cout << "grabFrame: dequeueing.."<< endl;
    
     
    dc1394_capture_dequeue(dcCam, policy, &dcFrame);
    //std::cout << "grabFrame: finished dequeueing.."<< endl;
    if (!dcFrame)
        return false; // Something bad happened
        
    // **** What are these? ****
#ifdef DC1394CAM_DRIVER_OPENCV_INSTALLED
    IplImage fhdr;
#endif //DC1394CAM_DRIVER_OPENCV_INSTALLED
    dc1394video_frame_t f = *dcFrame, *fc = &f;
    
    // Check to see if the buffer is full. This will typically occur repeatedly.
    if ((dcFrame->frames_behind >= nDMABufs - 1) && suppress_warnings == false) {
        std::cerr << "dc1394cam_driver Warning: Frames potentially dropped." << endl;
    }
    // Copy the current timestamp to prevtimestamp
    prevtimestamp = timestamp;
    // Copy the timestamp to variable for use in multicam grabFrame()
    timestamp = dcFrame->timestamp;
    // Copy the frames behind variable for use in multicam grabFrame()
    frames_behind = dcFrame->frames_behind;
    // Check if frame is corrupt
    if (dc1394_capture_is_frame_corrupt(dcCam, dcFrame) == DC1394_TRUE)
    {
        std::cerr << "dc1394cam_driver Warning: Frame corrupt!." << endl;
        if (dcFrame)
            dc1394_capture_enqueue(dcCam, dcFrame);
        return code;
    }
    
    // If image is color, set number of channels to three. If grey, 1.
    nch = isColor ? 3 : 1;

    // *************** Copied from OpenCV. Will clean up eventually....**********
    
    // If image is color, need to do some processing on it
    if (isColor)
    {
        if (!frameC)
            frameC = (dc1394video_frame_t*)calloc(1, sizeof(*frameC));
        frameC->color_coding = nch == 3 ? DC1394_COLOR_CODING_RGB8 : DC1394_COLOR_CODING_MONO8;
        dc1394_convert_frames(&f, frameC);
        dc1394_capture_enqueue(dcCam, dcFrame);
        dcFrame = 0;
        fc = frameC;
    }
    else {
        dc1394_capture_enqueue(dcCam, dcFrame);
    }
#ifdef DC1394CAM_DRIVER_OPENCV_INSTALLED
    // If image data hasn't been allocated yet, do so.
    // This should only be allocated once, but something is deallocating 
    // img so it needs to be reallocated on each grab....
    img = cvCreateImage(cvSize(fc->size[0], fc->size[1]), 8, nch);
    //std::cout << "grabFrame: creating img.."<< endl;
    
    // Copy data
    cvInitImageHeader(&fhdr, cvSize(fc->size[0], fc->size[1]), 8, nch);
    cvSetData(&fhdr, fc->image, fc->size[0]*nch);
    //std::cout << "grabFrame: finished copying data" << endl;
	// Swap R&B channels.               WHY?
    if (nch==3) {
        //std::cout << "grabFrame: starting cvconvertimage" << endl;
		cvConvertImage(&fhdr,&fhdr,CV_CVTIMG_SWAP_RB);
        //std::cout << "grabFrame: finished cvconvertimage" << endl;
	}
    
    // Copy the image data to the img container
    //std::cout << "grabFrame: starting cvcopy" << endl;
    cvCopy(&fhdr, img);
    //std::cout << "grabFrame: finished cvcopy" << endl;
    //cvReleaseImage(&img);
#endif //DC1394CAM_DRIVER_OPENCV_INSTALLED
    
#ifdef DC1394CAM_DRIVER_VXL_INSTALLED
    //vxlImg.clear();
    vil_image_view<unsigned char> image((unsigned char*)fc->image, fc->size[0], fc->size[1], nch, 1, 1, 1);
    vxlImg.deep_copy(image);
    // TODO: The clear() above does not clear memory properly. It is a memory leak. Needs fixing
    //vxlImg.clear();
#endif //DC1394CAM_DRIVER_VXL_INSTALLED
    
    // **************************************************************************
    code = true;
    return code;
}

#ifdef DC1394CAM_DRIVER_OPENCV_INSTALLED 
// Return the frame pointer to the user
IplImage* camCapture::retrieveFrame()
{
    return img;
}

// grabFrame and retrieveFrame in one call
IplImage* camCapture::queryFrame()
{
    if (grabFrame())
        return img;
    else return 0;
}
#endif //DC1394CAM_DRIVER_OPENCV_INSTALLED

// Value of index variable
int camCapture::getIndex() {
    return index;
}
// Value of open variable
bool camCapture::isOpen() {
    return opened;
}

// Value of open variable
bool camCapture::isStarted() {
    return started;
}
// Value of current frame timestamp
uint64_t camCapture::getTimeStamp() {
    return timestamp;
}
// Value of previous frame timestamp
uint64_t camCapture::getPrevTimeStamp() {
    return prevtimestamp;
}


// Value of current frames behind variable
unsigned int camCapture::getFramesBehind() {
    return frames_behind;
}

/****************** Set Camera Parameters ***********************/

// Set Firewire Mode
bool camCapture::setIsoSpeed(int speed) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set ISO Speed because camera is already "
                << "started. Set ISO Speed before attempting to grab frames." << endl;
        result = false;
    }
    else {
        switch (speed) {
            case 100:
            case 200:
            case 400:
            case 800:
            case 1600:
                isoSpeed = speed;
                result =  true;
                break;
            default:
                std::cerr << "dc1394cam_driver Error: Cannot set ISO speed of " << speed
                        << "MB/s. Firewire supports only 100, 200, 400, 800, 1600 or 3200 MB/s" << endl;
                result =  false;
        }
    }
    return result;
}

// Set frame width
bool camCapture::setWidth(int width) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame width because camera is already "
        << "started. Set frame width before attempting to grab frames." << endl;
        result = false;
    }
    else {
        frameWidth = width;
        result = true;
    }
    return result;
}

// Set frame height
bool camCapture::setHeight(int height) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame height because camera is already "
        << "started. Set frame height before attempting to grab frames." << endl;
        result = false;
    }
    else {
        frameHeight = height;
        result = true;
    }
    return result;
}


// Set frame width and height in one call
bool camCapture::setWidthAndHeight(int width, int height) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame width and height because camera is already "
        << "started. Set frame width and height before attempting to grab frames." << endl;
        result = false;
    }
    else {
        result = setWidth(width);
        if (!result) return result;
        else result = setHeight(height);
    }
    return result;
}


// Set framerate
bool camCapture::setFrameRate(std::string framerate) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set frame rate because camera is already "
        << "started. Set frame rate before attempting to grab frames." << endl;
        result = false;
    }
    else {
        fps = framerate;
        std::istringstream frame_rate_ss(fps);
        if (!(frame_rate_ss>>fps_float)) {
            std::cerr << "Error, could not convert " << frame_rate_ss.str() << " to double when setting the framerate. Future behaviour undefined.";
        } else {
            result = true;
        }
    }
    return result;
}

// **************************** DC1394 Specific Features ***********************************

// Set feature value
bool camCapture::setFeature(std::string featureString, int num) {
    bool result = false;
    if (isOpen()) { // If camera is open, set the feature immediately because we have direct access
        int featureIndex = 0;
        for(dc1394feature_t feature = DC1394_FEATURE_MIN; feature <= DC1394_FEATURE_MAX; 
                feature = (dc1394feature_t)(feature+1)) { 
            if (featureString == featuresStr[featureIndex]) { 
                // Mode is set to manual and we have found the right dc1394feature_t
                setDC1394Feature(dcCam, feature, featuresStr[featureIndex], DC1394_FEATURE_MODE_MANUAL, num);
                result = true;
                break;
            }
            featureIndex++;
        }
    } else { // If camera is not yet open, keep the information for when we open it.
        int featureIndex = 0;
        for(dc1394feature_t feature = DC1394_FEATURE_MIN; feature <= DC1394_FEATURE_MAX; 
                feature = (dc1394feature_t)(feature+1)) { 
            if (featureString == featuresStr[featureIndex]) { 
                // Mode is set to manual and we have found the right dc1394feature_t
                std::pair<std::string,int> featurepair(featureString,num);
                features_to_set.insert(featurepair);
                result = true;
                break;
            }
            featureIndex++;
        }

    }
    if (!result) {
        std::cerr << "dc1394cam_driver Error: Feature type \"" << featureString << "\" is unavailable. " << endl;
    }
    return result;
}


bool camCapture::setFeatureAbs(std::string featureString, float num) {
    bool result = false;
    if (isOpen()) { // If camera is open, set the feature immediately because we have direct access
        int featureIndex = 0;
        for(dc1394feature_t feature = DC1394_FEATURE_MIN; feature <= DC1394_FEATURE_MAX; 
                feature = (dc1394feature_t)(feature+1)) { 
            if (featureString == featuresStr[featureIndex]) { 
                // Mode is set to manual and we have found the right dc1394feature_t
                setDC1394FeatureAbs(dcCam, feature, featuresStr[featureIndex], DC1394_FEATURE_MODE_MANUAL, num);
                result = true;
                break;
            }
            featureIndex++;
        }
    } else { // If camera is not yet open, keep the information for when we open it.
        int featureIndex = 0;
        for(dc1394feature_t feature = DC1394_FEATURE_MIN; feature <= DC1394_FEATURE_MAX; 
                feature = (dc1394feature_t)(feature+1)) { 
            if (featureString == featuresStr[featureIndex]) { 
                // Mode is set to manual and we have found the right dc1394feature_t
                std::pair <std::string,float> featurepair(featureString,num);
                features_abs_to_set.insert(featurepair);
                result = true;
                break;
            }
            featureIndex++;
        }

    }
    if (!result) {
        std::cerr << "dc1394cam_driver Error: Feature type \"" << featureString << "\" is unavailable. " << endl;
    }
    return result;
}

// Set feature to automatic control
bool camCapture::setFeatureAuto(std::string featureString) {
    bool result = false;
    if (isOpen()) { // If camera is open, set the feature immediately because we have direct access
        int featureIndex = 0;
        for(dc1394feature_t feature = DC1394_FEATURE_MIN; feature <= DC1394_FEATURE_MAX; 
                feature = (dc1394feature_t)(feature+1)) { 
            if (featureString == featuresStr[featureIndex]) { 
                // Mode is set to manual and we have found the right dc1394feature_t
                setDC1394FeatureAbs(dcCam, feature, featuresStr[featureIndex], DC1394_FEATURE_MODE_AUTO, 0);
                result = true;
                break;
            }
            featureIndex++;
        }
    } else {
        // If not yet open, it will be set to auto on startup anyway, do nothing
        result = true;
    }
    if (!result) {
        std::cerr << "dc1394cam_driver Error: Feature type \"" << featureString << "\" is unavailable. " << endl;
    }
    return result;
}

// **************************** Point Grey Specific Features ***********************************

// Set feature value
bool camCapture::setPtGreyFeature(std::string featureString, int num) {
    bool result = false;
    if (isOpen()) { // If camera is open, set the feature immediately because we have direct access
        // Enable the max Shutter value
        if (featureString == "MaxShutter") {
            result = true;
            // Set the point grey specific maxShutterVal
            stringstream ss;
            std::cout << "Setting MaxShutterVal for camera to " << num << std::endl;

            uint32_t maxShutter = num;

            // AUTO_SHUTTER_RANGE
            uint64_t offset = 0x1098;
            uint32_t mask = 0x00000FFF;
            setRegister(dcCam, offset, maxShutter, mask);
        }
    } else { // If camera is not yet open, keep the information for when we open it.
        int featureIndex = 0;
        for(unsigned idx = 0; idx < ptGreyFeaturesStrSize; idx++) { 
            if (featureString == ptGreyFeaturesStr[idx]) {
                // Mode is set to manual and we have found the right Ptgrey feature
                std::pair<std::string,int> featurepair(featureString,num);
		        //cout << "inserting feature " << featureString << " with value " << num << " to ptgrey_features_to_set" << std::endl;
                ptgrey_features_to_set.insert(featurepair);
                result = true;
                break;
            }
            featureIndex++;
        }

    }
    if (!result) {
        std::cerr << "dc1394cam_driver Error: Point Grey specific feature type \"" << featureString << "\" is unavailable. " << endl;
    }
    return result;
}

// Set absolute feature value
bool camCapture::setPtGreyFeatureAbs(std::string featureString, float num) {
    bool result = false;
    if (isOpen()) { // If camera is open, set the feature immediately because we have direct access
        // Enable the max Shutter value
        if (featureString == "MaxShutter") {
            result = true;
            // Set the point grey specific maxShutterVal
            float maxShutterAbsValue = num;
            //std::cout << "Setting MaxShutterValAbs for camera to " << setprecision(5) << maxShutterAbsValue << " seconds." << std::endl;

            // Set the feature absolute value, so we can extract the relative value
            setDC1394FeatureAbs(dcCam, (dc1394feature_t)423, featuresStr[7], DC1394_FEATURE_MODE_MANUAL, maxShutterAbsValue);

            // Extract the non-absolute value
            uint32_t maxShutter;
            if (dc1394_feature_get_value(dcCam,(dc1394feature_t)423, &maxShutter) != DC1394_SUCCESS) {
              std::cerr << "Could not get Shutter value on camera when setting up absolute maximum shutter." << std::endl;
            }

            // Set Shutter back to auto
            setDC1394Feature(dcCam, (dc1394feature_t)423, featuresStr[7] , DC1394_FEATURE_MODE_AUTO, 0);

            // Now we can work as normal
            // AUTO_SHUTTER_RANGE
            uint64_t offset = 0x1098;
            uint32_t mask = 0x00000FFF;
            setRegister(dcCam, offset, maxShutter, mask);
        }
    } else { // If camera is not yet open, keep the information for when we open it.
        int featureIndex = 0;
        for(unsigned idx = 0; idx < ptGreyFeaturesStrSize; idx++) { 
            if (featureString == ptGreyFeaturesStr[idx]) {
                // Mode is set to manual and we have found the right Ptgrey feature
                std::pair<std::string,float> featurepair(featureString,num);
                ptgrey_features_abs_to_set.insert(featurepair);
                result = true;
                break;
            }
            featureIndex++;
        }

    }
    if (!result) {
        std::cerr << "dc1394cam_driver Error: Point Grey specific feature type \"" << featureString << "\" is unavailable. " << endl;
    }
    return result;
}

// Set feature to automatic control
bool camCapture::setPtGreyFeatureAuto(std::string featureString) {
    bool result = false;
    if (isOpen()) { // If camera is open, set the feature immediately because we have direct access
        // Disable the max Shutter value
        if (featureString == "MaxShutter") {
            result = true;
            
            // Set the point grey specific maxShutterVal
            uint32_t maxShutter = 4095;

            // AUTO_SHUTTER_RANGE
            uint64_t offset = 0x1098;
            uint32_t mask = 0x00000FFF;
            setRegister(dcCam, offset, maxShutter, mask);
        }
    } else {
        // If not yet open, it will be set to auto on startup anyway, do nothing
        result = true;
    }
    if (!result) {
        std::cerr << "dc1394cam_driver Error: Point Grey specific feature type \"" << featureString << "\" is unavailable. " << endl;
    }
    return result;
}


// Set number of DMA bufs
bool camCapture::setDMABufSize(int num) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set size of DMA buffer because camera is already "
        << "started. Set size of DMA buffer before attempting to grab frames." << endl;
        result = false;
    }
    else {
        nDMABufs = num;
        result = true;
    }
    return result;
}

// Set cleanup
bool camCapture::setCleanup(bool value) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set cleanup variable because camera is already "
        << "started. Set cleanup value before attempting to grab frames." << endl;
        result = false;
    }
    else {
        cleanup = value;
        result = true;
    }
    return result;
}

// Set suppress warnings variable
bool camCapture::setSuppressWarnings(bool value) {
    suppress_warnings = value;
    return true;
}

// Set color mode
bool camCapture::setColorMode(std::string colorMode) {
    bool result = false;
    if (isStarted()) {
        std::cerr << "dc1394cam_driver Error: Cannot set color mode because camera is already "
        << "open. Set color mode before attempting to grab frames." << endl;
        result = false;
    }
    else {
        // Make sure color mode is something that driver understands
        if (!isColorValid(prefColorMode)) {
            result = false;
        }
        else {
            prefColorMode = colorMode;
        }
        result = true;
    }
    return result;
}

// Set suppress warnings variable
bool camCapture::setGUID(uint64_t value) {
    bool result = false;
    if (isOpen()) {
        std::cerr << "dc1394cam_driver Error: Cannot set guid because camera is already "
        << "open. Set guid before calling open()." << endl;
        result = false;
    }
    else {
        guid = value;
        result = true;
    }
    return result;
}

/****************** Get Camera Parameters ***********************/

// Get Firewire Mode
int camCapture::getIsoSpeed(void) {
    return isoSpeed;
}
// Get frame width
int camCapture::getWidth(void) {
    return frameWidth;
}
// Get frame height
int camCapture::getHeight(void) {
    return frameHeight;
}
// Get framerate
std::string camCapture::getFrameRate(void) {
    return fps;
}
// Get framerate
float camCapture::getFrameRateFloat(void) {
    return fps_float;
}
// Get number of DMA bufs
int camCapture::getDMABufSize(void) {
    return nDMABufs;
}
// Get cleanup
bool camCapture::getCleanup(void) {
    return cleanup;
}
// Get suppress warnings variable
bool camCapture::getSuppressWarnings(void) {
    return suppress_warnings;
}
// Get color mode
std::string camCapture::getColorMode(void) {
    return prefColorMode;
}
// Get GUID variable
uint64_t camCapture::getGUID(void) {
    return guid;
}

/****************** VXL Functions ***********************/

#ifdef DC1394CAM_DRIVER_VXL_INSTALLED
// Return the frame pointer to the user
vil_image_view<vxl_byte> camCapture::retrieveVxlFrame(void) {
    return vxlImg;
}
// grabFrame and retrieveFrame in one call
vil_image_view<vxl_byte> camCapture::queryVxlFrame(void) {
    grabFrame();
    return vxlImg;
}

#endif //DC1394CAM_DRIVER_VXL_INSTALLED

/******************* Register setting functions ********************/
bool setRegister(dc1394camera_t* dcCam, uint64_t offset, uint32_t &value, uint32_t mask) {

    uint32_t origValue;
    
    if (dc1394_get_control_register(dcCam, offset, &origValue) != DC1394_SUCCESS)
    {
        std::cerr << "Can't get register 0x" << hex << offset << " for camera" << std::endl;
    }
    else
    {
        std::cerr << "Actual unmodified value of register 0x" << hex << offset 
	        << " for camera " << ": 0x" << hex << origValue << std::endl;
    }

    // Make sure the new value is correct according to the mask
    uint32_t modifiedVal = value & mask;
    
    if (modifiedVal != value) {
        std::cerr << "The requested value does not fit the mask!"<< std::endl;
        return false;
    }
    
    //cout << "Mask: 0x" << hex << mask << endl;
    //cout << "orig before mask: 0x" << hex << origValue << endl;
    // Clear out the old value
    origValue = origValue & ~mask;
    //cout << "orig after mask: 0x" << hex << origValue << endl;
    // Insert the new shutter value
    uint32_t newValue = origValue | value;
    std::cerr << "Desired modified value of register 0x" << hex << offset 
      << " for camera " << ": 0x" << hex << setfill('0') << setw(8) << newValue << std::endl;

    // Rewrite the value back
    if (dc1394_set_control_register(dcCam, offset, newValue) != DC1394_SUCCESS)
    {
        std::cerr << "Can't get register 0x" << hex << offset << " for camera." << std::endl;
    }
    else
    {
        
        uint32_t getValue;
        
        if (dc1394_get_control_register(dcCam, offset, &getValue) != DC1394_SUCCESS)
        {
            std::cerr << "Can't get register 0x" << hex << offset << " for camera." << std::endl;
        }
        std::cerr << "Actual modified value of register 0x" << hex << offset 
	  << " for camera: 0x" << hex << getValue << std::endl;
    }
    return true;
}



/****************** Feature Setting Functions ***********************/
// Set a feature
bool setDC1394Feature(dc1394camera_t* dcCam, dc1394feature_t feature, const char*& featureString, 
        dc1394feature_mode_t mode, unsigned int value) {
    dc1394bool_t present;
    dc1394_feature_is_present(dcCam , feature, &present); // Make sure the feature is present
    if (!present && mode == DC1394_FEATURE_MODE_MANUAL) { // Be quiet if we're setting the mode to auto
      std::cerr << featureString << " is not available on the " << dcCam->model 
            << ", cannot set manual value." << endl;
      return false;
    }
    // Make sure it's within the bounds
    unsigned int minVal, maxVal;
    dc1394_feature_get_boundaries(dcCam, feature, &minVal, &maxVal); 
    if (mode == DC1394_FEATURE_MODE_MANUAL && (value < minVal || value > maxVal)) {
      std::cerr << "Requested " << featureString << " value of " << value << " is outside available range of "
          << minVal << " to " << maxVal << " on the " << dcCam->model << endl;
      return false;
    }
    // Make sure it has manual mode
    if (mode == DC1394_FEATURE_MODE_MANUAL) {
        std::cerr << "Setting " << featureString << " to \"" << value << "\" on the " << dcCam->model << endl;
        // Set the feature to Manual and set the value
        if (dc1394_feature_set_mode(dcCam,feature,mode) != DC1394_SUCCESS) {
            std::cerr << "Could not set " << featureString << " to manual on the " << dcCam->model ;
            return false;
        }
        if (dc1394_feature_set_value(dcCam,feature,value) != DC1394_SUCCESS) {
            std::cerr << "Could not set " << featureString << " to value " << value << " on the " << dcCam->model << endl;
           return false;
        }
    } else if (mode == DC1394_FEATURE_MODE_AUTO) {
        // Ensure that feature is set to Auto
        //std::cerr << "Setting " << featureString << " to auto on the " << dcCam->model << endl;
        if (dc1394_feature_set_mode(dcCam,feature,mode)!= DC1394_SUCCESS) {
            std::cerr << "Could not set " << featureString << " to auto on the " << dcCam->model << endl;
            return false;
        }
    } else {
        std::cerr << "Could not set " << featureString << " to manual on the " << dcCam->model << endl;
        return false;
    }
    return true;
}


// Set a feature with absolute value
bool setDC1394FeatureAbs(dc1394camera_t* dcCam, dc1394feature_t feature, const char*& featureString, 
        dc1394feature_mode_t mode, float value) {
    dc1394bool_t present;
    dc1394_feature_is_present(dcCam , feature, &present); // Make sure the feature is present
    if (!present && mode == DC1394_FEATURE_MODE_MANUAL) {
      std::cerr << featureString << " is not available on the " << dcCam->model 
            << ", cannot set manual value." << endl;
      return false;
    }
    // Make sure it has absolute control
    dc1394_feature_has_absolute_control(dcCam, feature, &present); 
    if (mode == DC1394_FEATURE_MODE_MANUAL && !present) {
      std::cerr << "Abolute value for " << featureString << " is not available on the " << dcCam->model << ", cannot set manual value." << endl;
      return false;
    }
    else if (!present) {
        return false;
    }
    // Make sure it's within the bounds
    float minVal, maxVal;
    dc1394_feature_get_absolute_boundaries(dcCam, feature, &minVal, &maxVal); 
    if (mode == DC1394_FEATURE_MODE_MANUAL && (value < minVal || value > maxVal)) {
      std::cerr << "Requested absolute " << featureString << " value of " << value << " is outside available range of "
          << minVal << " to " << maxVal << " on the " << dcCam->model << endl;
      return false;
    }
    // Make sure it has manual mode
    if (mode == DC1394_FEATURE_MODE_MANUAL) {
        std::cerr << "Setting " << featureString << " to \"" << value << "\" on the " << dcCam->model << endl;
        if (dc1394_feature_set_mode(dcCam,feature,mode) != DC1394_SUCCESS) {
            std::cerr << "Could not set " << featureString << " to manual on the " << dcCam->model << endl;
            return false;
        }
        if (dc1394_feature_set_absolute_control(dcCam,feature,DC1394_ON) != DC1394_SUCCESS) {
            std::cerr << "Could not set absolute control on feature" << featureString << " to 'on' on the " << dcCam->model << endl;
            return false;
        }
        if (dc1394_feature_set_absolute_value(dcCam,feature,value) != DC1394_SUCCESS) {
            std::cerr << "Could not set " << featureString << " to value " << value << " on the " << dcCam->model << endl;
            return false;
        }
    } else if (mode == DC1394_FEATURE_MODE_AUTO) {
        // Ensure that feature is set to auto
        //ss << "Setting " << featureString << " to auto on the " << dcCam->model << endl;
        if (dc1394_feature_set_absolute_control(dcCam,feature,DC1394_OFF) != DC1394_SUCCESS) {
            std::cerr << "Could not set absolute control on feature" << featureString << " to 'off' on the " << dcCam->model << endl;
            return false;
        }
        if (dc1394_feature_set_mode(dcCam,feature,mode)!= DC1394_SUCCESS) {
            std::cerr << "Could not set " << featureString << " to auto on the " << dcCam->model << endl;
            return false;
        }
    } else {
        std::cerr << "Could not set " << featureString << " to manual on the " << dcCam->model
                << ". It is not supported" << endl;
        return false;
    }
    return true;
}

#undef DC1394CAM_DRIVER_VXL_INSTALLED
#undef DC1394CAM_DRIVER_OPENCV_INSTALLED
